/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Gyro;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.vision.Target;
import edu.wpi.first.wpilibj.vision.TrackerDashboard;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Watchdog;
public class GyroRobot extends SimpleRobot {

    private Gyro gyro = new Gyro(1);
    private double kVisionThreshold = .01;
    private Joystick leftStick = new Joystick(1);
    private Joystick rightStick = new Joystick(2);
    private RobotDrive drive = new RobotDrive(1, 2, 3, 4);
    private AxisCamera cam = AxisCamera.getInstance();
    private TrackerDashboard trackerDashboard = new TrackerDashboard();
    private Joystick leftstick = new Joystick(1);
    private Joystick rightstick = new Joystick(2);
    private Watchdog dog;
    public GyroRobot() {
        gyro.reset();
        gyro.setSensitivity(0.005);
        dog = dog.getInstance();
    }

    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        double angle = 0;
        while (true && isOperatorControl() && isEnabled()) //loop until change
        {
            if (leftstick.getTrigger()) {
                goToAngle();
            }
            System.out.println(gyro.getAngle());
            drive.tankDrive(leftstick, rightstick);
            dog.feed();
            Timer.delay(.005);

        }

    }

    public void goToAngle() {
        System.out.println("goToAngle");
        double currentAngle = gyro.getAngle();
        double finalAngle = 360;
        while (gyro.getAngle() < (currentAngle + finalAngle)) {
            System.out.println(gyro.getAngle());
            drive.setLeftRightMotorSpeeds(-.5, .5);
            dog.feed();
        }
                    drive.setLeftRightMotorSpeeds(0, 0);

    }
}
